
//*
//-------------------------------------------------------
//Kalman滤波，8MHz的处理时间约1.8ms；
//-------------------------------------------------------
static float angle, angle_dot; //外部需要引用的变量  角度（angle）、角速度（angle_dot）
//-------------------------------------------------------
  /*
   * Q is a 2x2 matrix of the covariance. Because we
   * assume the gyro and accelerometer noise to be independent
   * of each other, the covariances on the / diagonal are 0.
   *
   * Covariance Q, the process noise, from the assumption
   *    x = F x + B u + w
   * with w having a normal distribution with covariance Q.
   * (covariance = E[ (X - E[X])*(X - E[X])' ]
   * We assume is linear with dt
   * Q是一个2x2的矩阵，协方差。因为我们承担的陀螺仪和加速度计噪声相互独立，/对角协方差为0。
   * 假设协方差Q的过程噪声，与W有一个正态分布的方差问
   */
//Q_angle和Q_gyro分别是加速度计和陀螺仪测量的协方差,其数值代表卡尔曼滤波器对其传感器数据
//的信任程度,值越小,表明信任程度越高.在该系统中陀螺仪的值更为接近准确值,因此取q_gyro的值小于q_acce的值.
static const float Q_angle=0.001, Q_gyro=0.003, 
  /*
   * Covariance R, our observation noise (from the accelerometer)
   * Also assumed to be linair with dt
   * 协方差为R，我们的观测噪声（加速度计），也可认为是与DT linair
   */
/*
 * R represents the measurement covariance noise.  In this case,
 * it is a 1x1 matrix that says that we expect 0.3 rad jitter
 * from the accelerometer.
 * R表示测量方差噪声。在这种情况下，它是一个1x1的矩阵，说，我们预期从0.3 RAD抖动的加速度。
 */   //.3 default
      R_angle=0.3, 
//  dt的取值为kalman滤波器采样周期
      dt=0.028;

static float P[2][2] = {
                          {  1, 0   },
                          {  0, 1   }
                        };
static float Pdot[4] ={
  0,0,0,0};

static const char C_0 = 1;

  /* These variables represent our state matrix x */
static float q_bias, angle_err, 
  /* Our error covariance matrix  我们的协方差矩阵 */
  PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//-------------------------------------------------------
/*
  angle_m:经过 atan2(ax, ay)方法计算的偏角,弧度值
  gyro_m:经过初步减去零点的陀螺仪角速度值,弧度值
*/
void Kalman_Filter(float angle_m,float gyro_m)//gyro_m:gyro_measure
{
    
    angle+=(gyro_m-q_bias) * dt;//先验估计
    
    Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' 先验估计误差协方差的微分
    Pdot[1]=- P[1][1];
    Pdot[2]=- P[1][1];
    Pdot[3]=Q_gyro;
    
    P[0][0] += Pdot[0] * dt;// Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差
    P[0][1] += Pdot[1] * dt;
    P[1][0] += Pdot[2] * dt;
    P[1][1] += Pdot[3] * dt;
    
    
    angle_err = angle_m - angle;//zk-先验估计
    
    
    PCt_0 = C_0 * P[0][0];
    PCt_1 = C_0 * P[1][0];
    
    E = R_angle + C_0 * PCt_0;
    
    K_0 = PCt_0 / E;//Kk
    K_1 = PCt_1 / E;
    
    t_0 = PCt_0;
    t_1 = C_0 * P[0][1];
    
    P[0][0] -= K_0 * t_0;//后验估计误差协方差
    P[0][1] -= K_0 * t_1;
    P[1][0] -= K_1 * t_0;
    P[1][1] -= K_1 * t_1;
    
    
    angle+= K_0 * angle_err;//后验估计
    q_bias+= K_1 * angle_err;//后验估计
    angle_dot = gyro_m-q_bias;//输出值（后验估计）的微分 = 角速度
}
//*/
